# -*- -*- -*- -*- -*- -*- -*-
# @Author  : mengzhe
# @Time    : 2025/4/28 09:52
# @File    : radar.py
# @function:
# @input   :
# @output  :
# @Software: PyCharm 
# -*- -*- -*- -*- -*- -*- -*-

import socket
import struct
import time
import json
import numpy as np
from datetime import datetime
from typing import Tuple, Dict, Optional, List


class ASSRRadarServer:
    """
    ASSR-S形变监测雷达服务器端实现
    """

    # def __init__(self, host: str = '0.0.0.0', port: int = 3000):
    def __init__(self, host: str = '192.168.1.135', port: int = 3000):
        """
        初始化雷达服务器

        :param host: 服务器IP地址
        :param port: 服务器端口
        """
        self.host = host
        self.port = port
        self.server_socket = None
        self.client_socket = None
        self.client_address = None
        self.command_counter = 0  # 命令序号计数器
        self.device_id = None  # 雷达设备ID

        self.command =None #雷达的指令

        # 雷达状态缓存
        self.radar_status = {
            'device_id': None,
            'work_state': 0,  # 0:停止工作, 1:工作中, 2:初始化中, 3:休眠中
            'angle': 0.0,
            'error_code': 0,
            'voltage': 0.0,
            'current': 0.0,
            'radar_temp': 0.0,
            'processor_temp': 0.0
        }

        # 场景参数
        self.scene_params = {
            'ro_rate': 20,  # 转台转速
            'mode': 0,  # 0:自主模式, 1:受控模式
            'rot_ang_bgn': 0.0,  # 起始扫描角
            'rot_ang_end': 180.0,  # 结束扫描角
            'rng_min': 10,  # 最近探测距离(米)
            'rng_max': 1000,  # 最远探测距离(米)
            'scan_int': 0,  # 扫描间隔
            'backup': 0,  # 备用
            'atm_switch': 1,  # 大气开关
            'filter_level': 5,  # 滤波强度
            'ps_td': 15  # ps筛选门限
        }

        # 输出控制参数
        self.output_params = {
            'scat_image_en': 1,  # 强度点云有效
            'scat_image_dec': 0,  # 强度点云抽取比
            'defo_image_en': 1,  # 形变点云有效
            'defo_image_dec': 0,  # 形变点云抽取比
            'scan_int': 0,  # 扫描间隔(秒)
            'backup0': 0,  # 备用
            'backup1': 0,  # 备用
            'backup2': 0,  # 备用
        }

        # 数据存储
        self.scatter_data = []  # 强度点云数据
        self.deformation_data = []  # 形变点云数据

    def start_server(self):
        """启动TCP服务器"""
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.server_socket.bind((self.host, self.port))
        self.server_socket.listen(1)
        # print(f"服务器已启动，监听 {self.host}:{self.port}")
        current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
        print(f"[Radar][{current_time}] Radar started and listening: {self.host}:{self.port}")

        # 等待雷达连接
        self.client_socket, self.client_address = self.server_socket.accept()
        current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
        print(f"[Radar][{current_time}] Radar connected {self.client_address}")
        # print(f"雷达已连接: {self.client_address}")

    def stop_server(self):
        """停止服务器"""
        if self.client_socket:
            self.client_socket.close()
        if self.server_socket:
            self.server_socket.close()
        print(f"[Radar][{datetime.now().strftime('%H:%M:%S.%f')[:-3]}] Radar Server stoped")

    def clear_message_queue(self, timeout: float = 5.0):
        """ 清空消息队列"""
        if not self.client_socket:
            return None

        # 设置超时
        self.client_socket.settimeout(timeout)

        try:
            # 读取冗余的消息队列
            message = self.client_socket.recv(1024 * 100)
            print(f"接收冗余消息: {message.hex()}")
            if len(message) > 0:
                return None
            return None
        except socket.timeout:
            print("接收响应超时")
            return None
        finally:
            self.client_socket.settimeout(None)

    def _send_command(self, command_code: int, ext_code: int = 0, data: bytes = b'') -> int:
        """
        发送命令到雷达

        :param command_code: 命令码
        :param ext_code: 扩展码
        :param data: 命令数据
        :return: 命令序号
        """
        if not self.client_socket:
            raise ConnectionError("雷达未连接")

        # 构建命令头
        command_header = 0x5A5A  # 主控命令头

        # 设备ID (假设为0，实际应根据雷达ID设置)
        device_id = self.device_id

        # 状态域 (指令类型为0:指令)
        status_field = bytes([0]) + self.command_counter.to_bytes(4, 'little') + bytes(11)

        # 数据长度
        data_length = len(data)

        # 构建完整命令
        command = (
                struct.pack('<H', command_header) +  # 命令头
                struct.pack('<I', device_id) +  # 设备ID
                bytes([command_code]) +  # 命令码
                bytes([ext_code]) +  # 扩展码
                status_field +  # 状态域
                struct.pack('<I', data_length) +  # 数据长度
                data  # 数据内容
        )

        # print(f"发送指令: {command.hex()}")
        # print(f"发送指令: {command}")
        current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
        print(f"[Radar][{current_time}] Send data: {command.hex()}")

        # 发送命令
        self.client_socket.sendall(command)

        return command

    def _parse_response(self, cmd_send_radar: int, timeout: float = 5.0) -> Optional[Dict]:
        """
        解析雷达响应

        :param cmd_send_radar: 输入指令
        :param timeout: 超时时间(秒)
        :return: 解析后的响应字典，或None(超时)
        """
        if not self.client_socket:
            return None

        # 设置超时
        self.client_socket.settimeout(timeout)

        # 尝试次数
        max_retries = 3
        retry_count = 0

        while retry_count < max_retries:
            # try:
            # 读取响应头 (前32字节)
            response_header = self.client_socket.recv(28)
            # print(f"接收响应: {response_header.hex()}")
            if len(response_header) < 28:
                continue

            # 解析响应头
            command_header = struct.unpack('<H', response_header[:2])[0]
            device_id = struct.unpack('<I', response_header[2:6])[0]
            command_code = response_header[6]
            ext_code = response_header[7]
            status_field = response_header[8:24]
            data_length = struct.unpack('<I', response_header[24:28])[0]

            # print(f"响应头解析: 命令头={hex(command_header)}, 设备ID={device_id}, 命令码={command_code}, 扩展码={ext_code}, 状态域={status_field.hex()}, 数据长度={data_length}")

            # 验证命令头
            if response_header[0:2]!=b'\x3c\x3c':  # 雷达响应头
                print(f"无效的响应头: {response_header.hex()}")
                retry_count += 1
                continue


            # 检查指令编码是否一致
            if response_header[3:7]!=cmd_send_radar[3:7] and cmd_send_radar[9:12]!=response_header[9:12]:
                print(f"指令和反馈不一致: {response_header.hex()} 不同于{cmd_send_radar.hex()}，丢弃该响应")
                if data_length > 0:
                    _ = self.client_socket.recv(data_length)
                retry_count += 1
                continue


            data = b''
            if data_length > 0:
                # 循环接收数据，直到达到或超过所需长度
                while len(data) < data_length:
                    remaining = data_length - len(data)
                    chunk = self.client_socket.recv(min(4096, remaining))
                    if not chunk:  # 连接关闭或超时
                        print(f"连接关闭，数据不完整，期望 {data_length} 字节，仅收到 {len(data)} 字节")
                        break
                    data += chunk

                # 检查是否收到了足够的数据
                if len(data) < data_length:
                    print(f"数据不完整，期望 {data_length} 字节，收到 {len(data)} 字节")
                    continue


            # 解析状态域
            cmd_type = status_field[0]  # 0:指令, 1:响应
            cmd_seq = int.from_bytes(status_field[1:5], 'little')
            exec_status = status_field[5] if cmd_type == 1 else 0
            # print(f"接收响应: {response_header.hex()}{data.hex()}")
            # print(f"接收响应: {response_header}{data}")

            current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
            print(f"[Radar][{current_time}] Received data: {response_header.hex()}{data.hex()}")

            return {
                'command':response_header+data,
                'response_header':response_header,
                'command_header': command_header,
                'device_id': device_id,
                'command_code': command_code,
                'ext_code': ext_code,
                'cmd_type': cmd_type,
                'cmd_seq': cmd_seq,
                'exec_status': exec_status,
                'data_length': data_length,
                'data': data
            }

            # except socket.timeout:
            #     retry_count += 1
            #     print(f"接收响应超时，重试次数: {retry_count}/{max_retries}")
            #     if retry_count >= max_retries:
            #         print("达到最大重试次数，接收失败")
            #         return response_header
            # finally:
            #     self.client_socket.settimeout(None)

        # print("达到最大重试次数，接收失败")
        # return response_header

    def set_scene_params(self, cmd_data):
        """
        设置场景参数

        :param params: 场景参数字典
        """
        # 发送设置场景参数命令 (命令码0x00, 扩展码0x00)
        cmd_seq = self._send_command(0x00, 0x00, cmd_data)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            print("场景参数设置成功")
            return response['command']
        else:
            print("场景参数设置失败")

    def set_output_params(self, cmd_data):
        """
        设置输出控制参数
        """

        # 发送设置输出参数命令 (命令码0x00, 扩展码0x01)
        cmd_seq = self._send_command(0x00, 0x01, cmd_data)
        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            print("输出参数设置成功")
            return response['command']
        else:
            print("输出参数设置失败")

    def query_scene_params(self) -> Optional[Dict]:
        """
        查询场景参数

        :return: 场景参数字典，或None(查询失败)
        """
        # 发送查询场景参数命令 (命令码0x01, 扩展码0x00)
        cmd_send_radar = self._send_command(0x01, 0x00)
        print(f"发送查询场景参数命令bytes: {cmd_send_radar}")
        # 等待响应
        response = self._parse_response(cmd_send_radar)
        if response and response['exec_status'] == 0x00:
            data = response['data']
            # print(f"查询场景参数成功: {data.hex()}")
            if len(data) >= 30:  # 确保数据长度足够
                scene_params = {
                    'ro_rate': struct.unpack('<h', data[0:2])[0],
                    'mode': struct.unpack('<h', data[2:4])[0],
                    'rot_ang_bgn': struct.unpack('<f', data[4:8])[0],
                    'rot_ang_end': struct.unpack('<f', data[8:12])[0],
                    'rng_min': struct.unpack('<I', data[12:16])[0],
                    'rng_max': struct.unpack('<I', data[16:20])[0],

                    'scan_int': struct.unpack('<I', data[20:24])[0],
                    'back_up': struct.unpack('<f', data[24:28])[0],

                    'atm_switch': struct.unpack('<h', data[28:30])[0],
                    'filter_level': struct.unpack('<h', data[30:32])[0],
                    'ps_td': struct.unpack('<h', data[32:34])[0]
                }
                # self.scene_params.update(scene_params)
                print(f"查询场景参数成功: {scene_params}")
                # print("场景参数:", {data})
                return response['command']
                # return data
        return None


    def query_output_params(self) -> Optional[Dict]:
        """
        查询输出控制参数

        :return: 输出参数字典，或None(查询失败)
        """
        # 发送查询输出参数命令 (命令码0x01, 扩展码0x01)
        cmd_send_radar = self._send_command(0x01, 0x01)

        # 等待响应
        response = self._parse_response(cmd_send_radar)
        if response and response['exec_status'] == 0x00:
            data = response['data']
            if len(data) >= 24:  # 确保数据长度足够
                output_params = {
                    'scat_image_en': struct.unpack('<h', data[0:2])[0],
                    'scat_image_dec': struct.unpack('<h', data[2:4])[0],
                    'defo_image_en': struct.unpack('<h', data[4:6])[0],
                    'defo_image_dec': struct.unpack('<h', data[6:8])[0],
                    'scan_int': struct.unpack('<I', data[8:12])[0],
                    'back_up0': struct.unpack('<f', data[12:16])[0],
                    'back_up1': struct.unpack('<f', data[16:20])[0],
                    'back_up2': struct.unpack('<f', data[20:24])[0]
                }
                # self.output_params.update(output_params)
                print(f"[Radar][{datetime.now().strftime('%H:%M:%S.%f')[:-3]}] Output params{output_params}")

                return response['command']
        return None

    def query_status(self) -> Optional[Dict]:
        """
        查询雷达状态

        :return: 状态字典，或None(查询失败)
        """
        # 发送查询状态命令 (命令码0x02, 扩展码0x00)
        print("查询雷达状态")
        cmd_send_radar = self._send_command(0x02, 0x00)

        # 等待响应
        # print("发送完毕")
        response = self._parse_response(cmd_send_radar)
        # print("接收完毕")
        # if response and response['cmd_seq'] == cmd_send_radar and response['exec_status'] == 0x00:
        if response and response['exec_status'] == 0x00:
            data = response['data']
            if len(data) >= 30:  # 确保数据长度足够
                status = {
                    'device_id': struct.unpack('<I', data[0:4])[0],
                    'production_date': struct.unpack('<I', data[4:8])[0],
                    'voltage': struct.unpack('<e', data[8:10])[0],  # float16
                    'current': struct.unpack('<e', data[10:12])[0],  # float16
                    'radar_temp': struct.unpack('<e', data[12:14])[0],  # float16
                    'processor_temp': struct.unpack('<e', data[14:16])[0],  # float16
                    'radar_init': struct.unpack('<h', data[16:18])[0],
                    'work_state': struct.unpack('<h', data[18:20])[0],
                    'angle': struct.unpack('<e', data[20:22])[0],  # float16
                    'error_code': struct.unpack('<h', data[22:24])[0],
                    'algo_version': data[24:28].decode('ascii'),
                    'ctrl_version': data[28:32].decode('ascii')
                }
                # self.radar_status.update(status)
                # print("雷达状态:", status)
                current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
                print(f"[Radar][{current_time}] Radar status: {status}")
                return response['command']
        return None

    def start_work(self) -> bool:
        """
        开始工作

        :return: 是否成功
        """
        # 发送开始工作命令 (命令码0x03, 扩展码0x00)
        cmd_seq = self._send_command(0x03, 0x00)
        # cmd_seq = self._send_default_command(0x03,0x00)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            # if response and response['cmd_seq'] == cmd_seq:
            self.radar_status['work_state'] = 1
            print("雷达开始工作")
            return response['command']
        print("雷达启动失败")
        return response['response_header']

    def stop_work(self) -> bool:
        """
        停止工作

        :return: 是否成功
        """
        # 发送停止工作命令 (命令码0x04, 扩展码0x00)
        cmd_seq = self._send_command(0x04, 0x00)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            self.radar_status['work_state'] = 0
            print("雷达停止工作")
            return response['command']
        print("雷达停止失败")
        return response['response_header']

    def reboot(self) -> bool:
        """
        重启雷达

        :return: 是否成功
        """
        # 发送重启命令 (命令码0x05, 扩展码0x00)
        cmd_seq = self._send_command(0x05, 0x00)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            self.radar_status['work_state'] = 2  # 初始化中
            print("雷达正在重启")
            return response['command']
        print("雷达重启失败")
        return response['response_header']

    def sleep(self) -> bool:
        """
        雷达休眠

        :return: 是否成功
        """
        # 发送休眠命令 (命令码0x07, 扩展码0x00)
        cmd_seq = self._send_command(0x07, 0x00)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            self.radar_status['work_state'] = 3  # 休眠中
            return response['command']
        print("雷达休眠失败")
        return response['response_header']

    def wakeup(self) -> bool:
        """
        雷达唤醒

        :return: 是否成功
        """
        # 发送唤醒命令 (命令码0x07, 扩展码0x01)
        cmd_seq = self._send_command(0x07, 0x01)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            self.radar_status['work_state'] = 0  # 停止工作
            print("雷达已唤醒")
            return response['command']
        print("雷达唤醒失败")
        return response['response_header']

    def get_log_list(self) -> Optional[List[Dict]]:
        """
        获取日志列表

        :return: 日志列表，或None(获取失败)
        """
        # 发送获取日志列表命令 (命令码0x08, 扩展码0x00)
        cmd_seq = self._send_command(0x08, 0x00)

        # 等待响应
        response = self._parse_response(cmd_seq)
        if response and response['exec_status'] == 0x00:
            data = response['data'].decode('utf-8')
            log_entries = data.split(',')
            logs = []
            for i in range(0, len(log_entries), 2):
                if i + 1 < len(log_entries):
                    logs.append({
                        'name': log_entries[i],
                        'size': int(log_entries[i + 1])
                    })
            # print("日志列表:", logs)
            # 打印日志
            for idx, log in enumerate(logs, 1):
                # 每行显示两个日志项，使用制表符分隔
                print(f"{idx}. {log['name']}: {log['size']}")
            print(f"日志列表获取成功 {response['command']} ")
            print(f"日志列表获取成功 {response['command'].hex()} ")
            return response['command']
        return None


    def get_log(self, cmd_data) -> Optional[bytes]:
        """
        获取日志内容

        :param log_name: 日志名称
        :param offset: 日志字节偏移
        :return: 日志内容，或None(获取失败)
        """
        # 构造请求数据 (日志名称,日志字节偏移)
        offset = cmd_data
        # data = f"{log_name},{0}".encode('utf-8')
        # data = f"0000".encode('utf-8')

        # 发送获取日志命令 (命令码0x08, 扩展码0x01)
        cmd_seq = self._send_command(0x08, 0x01, cmd_data)
        print("请求日志内容")
        # 等待响应
        response = self._parse_response(cmd_seq)
        print("获取到日志内容")
        if response and response['exec_status'] == 0x00:
            print("日志内容:", response['command'].hex())
            return response['command']
        return None

    def handle_radar_data(self, data: Dict):
        """
        处理雷达主动上报的数据

        :param data: 解析后的雷达数据
        """
        if data['command_code'] == 0x40:  # 雷达主动上报数据
            if data['ext_code'] == 0x01:  # 强度点云数据
                print("强度点云数据")
                self._process_scatter_data(data['data'])
            elif data['ext_code'] == 0x02:  # 形变点云数据
                print("形变点云数据")
                self._process_deformation_data(data['data'])
            elif data['ext_code'] == 0x00:  # 雷达信息
                print("雷达主动上报")
                self._process_radar_info(data['data'])
            elif data['ext_code'] == 0x06:  # 请求授时
                print("请求授时")
                self._respond_to_time_request()

    def handle_cmd_radar_data(self, cmd_headar,data):
        """
        处理雷达主动上报的数据
        """
        if cmd_headar[6] == 0x40:  # 雷达主动上报数据
            current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
            if cmd_headar[7] == 0x01:  # 强度点云数据
                print(f"[Radar][{current_time}] 雷达强度点云")
            elif cmd_headar[7] == 0x02:  # 形变点云数据
                print(f"[Radar][{current_time}] 雷达形变点云")
            elif cmd_headar[7] == 0x00:  # 雷达信息
                print(f"[Radar][{current_time}] 雷达主动上报")
            elif cmd_headar[7] == 0x06:  # 请求授时
                print(f"[Radar][{current_time}] 雷达请求授时")


    def _process_scatter_data(self, data: bytes):
        """
        处理强度点云数据

        :param data: 原始数据
        """
        try:
            # 解析强度点云数据头
            if len(data) < 32:
                print("强度点云数据长度不足")
                return

            scan_num = struct.unpack('<i', data[0:4])[0]  # 帧序号
            timestamp = struct.unpack('<i', data[4:8])[0]  # 时间戳
            rng_res = struct.unpack('<f', data[8:12])[0]  # 距离像素间隔(米)
            rng_num = struct.unpack('<i', data[12:16])[0]  # 距离像素点数量
            ang_res = struct.unpack('<f', data[16:20])[0]  # 角度像素间隔(弧度)
            ang_num = struct.unpack('<i', data[20:24])[0]  # 角度像素点数量
            img_mean_amp = struct.unpack('<f', data[24:28])[0]  # 强度点云幅度均值
            sca_im_size = struct.unpack('<i', data[28:32])[0]  # 压缩后数据大小

            scatter_header = {
                'scan_num': scan_num,
                'timestamp': timestamp,
                'rng_res': rng_res,
                'rng_num': rng_num,
                'ang_res': ang_res,
                'ang_num': ang_num,
                'img_mean_amp': img_mean_amp
            }
            print(f"强度图文件头{scatter_header}")

            # 检查数据长度
            if len(data) < 32 + sca_im_size:
                print("强度点云数据不完整")
                return

            # 解压缩数据
            compressed_data = data[32:]

            # 存储数据
            scatter_info = {
                'scan_num': scan_num,
                'timestamp': timestamp,
                'rng_res': rng_res,
                'rng_num': rng_num,
                'ang_res': ang_res,
                'ang_num': ang_num,
                'img_mean_amp': img_mean_amp,
                'data': compressed_data
            }
            self.scatter_data.append(scatter_info)

            # 保存到文件
            self._save_scatter_data(scatter_info)

            print(f"接收到强度点云数据: 帧号 {scan_num}, 时间 {datetime.fromtimestamp(timestamp)}")

        except Exception as e:
            print(f"处理强度点云数据出错: {e}")

    def _process_deformation_data(self, data: bytes):
        """
        处理形变点云数据

        :param data: 原始数据
        """
        try:
            # 解析形变点云数据头
            if len(data) < 28:
                print("形变点云数据长度不足")
                return

            scan_num = struct.unpack('<i', data[0:4])[0]  # 帧序号
            timestamp = struct.unpack('<i', data[4:8])[0]  # 时间戳
            rng_res = struct.unpack('<f', data[8:12])[0]  # 距离像素间隔(米)
            rng_num = struct.unpack('<i', data[12:16])[0]  # 距离像素点数量
            ang_res = struct.unpack('<f', data[16:20])[0]  # 角度像素间隔(弧度)
            ang_num = struct.unpack('<i', data[20:24])[0]  # 角度像素点数量
            defo_im_size = struct.unpack('<i', data[24:28])[0]  # 压缩后数据大小

            # 检查数据长度
            if len(data) < 28 + defo_im_size:
                print("形变点云数据不完整")
                return

            # 解压缩数据
            compressed_data = data[28:28 + defo_im_size]

            # 存储数据
            deformation_info = {
                'scan_num': scan_num,
                'timestamp': timestamp,
                'rng_res': rng_res,
                'rng_num': rng_num,
                'ang_res': ang_res,
                'ang_num': ang_num,
                'data': compressed_data
            }
            self.deformation_data.append(deformation_info)

            # 保存到文件
            self._save_deformation_data(deformation_info)

            print(f"接收到形变点云数据: 帧号 {scan_num}, 时间 {datetime.fromtimestamp(timestamp)}")

        except Exception as e:
            print(f"处理形变点云数据出错: {e}")

    def _process_radar_info(self, data: bytes):
        """
        处理雷达信息

        :param data: 原始数据
        """
        try:
            # 解析雷达信息
            print("雷达主动上报")

            if len(data) >= 30:  # 确保数据长度足够
                status = {
                    'device_id': struct.unpack('<I', data[0:4])[0],
                    'production_date': struct.unpack('<I', data[4:8])[0],
                    'voltage': struct.unpack('<e', data[8:10])[0],  # float16
                    'current': struct.unpack('<e', data[10:12])[0],  # float16
                    'radar_temp': struct.unpack('<e', data[12:14])[0],  # float16
                    'processor_temp': struct.unpack('<e', data[14:16])[0],  # float16
                    'radar_init': struct.unpack('<h', data[16:18])[0],
                    'work_state': struct.unpack('<h', data[18:20])[0],
                    'angle': struct.unpack('<e', data[20:22])[0],  # float16
                    'error_code': struct.unpack('<h', data[22:24])[0],
                    'algo_version': data[24:28].decode('ascii'),
                    'ctrl_version': data[28:32].decode('ascii')
                }
                print(f"雷达状态更新: {status}")


        except Exception as e:
            print(f"处理雷达信息出错: {e}")

    def _respond_to_time_request(self):
        """
        响应雷达的授时请求
        """
        # 获取当前时间戳 (从1970年1月1日开始的秒数)
        timestamp = int(time.time())

        # 发送授时响应 (命令码0x40, 扩展码0x06)
        # cmd_seq = self._send_command(0x40, 0x06, struct.pack('<I', timestamp))

        print(f"已响应授时请求: 时间戳 {timestamp}")

    def _save_scatter_data(self, data: Dict):
        """
        保存强度点云数据到文件

        :param data: 强度点云数据
        """
        try:
            timestamp_str = datetime.fromtimestamp(data['timestamp']).strftime('%Y%m%d_%H%M%S')
            filename = f"scatter_{timestamp_str}_scan{data['scan_num']}.npz"

            np.savez_compressed(
                filename,
                scan_num=data['scan_num'],
                timestamp=data['timestamp'],
                rng_res=data['rng_res'],
                rng_num=data['rng_num'],
                ang_res=data['ang_res'],
                ang_num=data['ang_num'],
                img_mean_amp=data['img_mean_amp'],
                data=data['data']
            )

            print(f"强度点云数据已保存到 {filename}")
        except Exception as e:
            print(f"保存强度点云数据失败: {e}")

    def _save_deformation_data(self, data: Dict):
        """
        保存形变点云数据到文件

        :param data: 形变点云数据
        """
        try:
            timestamp_str = datetime.fromtimestamp(data['timestamp']).strftime('%Y%m%d_%H%M%S')
            filename = f"deformation_{timestamp_str}_scan{data['scan_num']}.npz"

            np.savez_compressed(
                filename,
                scan_num=data['scan_num'],
                timestamp=data['timestamp'],
                rng_res=data['rng_res'],
                rng_num=data['rng_num'],
                ang_res=data['ang_res'],
                ang_num=data['ang_num'],
                data=data['data']
            )

            print(f"形变点云数据已保存到 {filename}")
        except Exception as e:
            print(f"保存形变点云数据失败: {e}")

    def listen_for_data(self):
        """
        监听雷达主动上报的数据
        """
        if not self.client_socket:
            print("雷达未连接")
            return

        print("开始监听雷达数据...")
        self.client_socket.settimeout(1.0)  # 设置超时以便可以检查是否需要退出

        try:
            while True:
                try:
                    # 读取数据头 (前28字节)
                    header = self.client_socket.recv(28)
                    # print(f"接收指令头: {header.hex()}")
                    if not header:
                        print("雷达连接已关闭")
                        break

                    if len(header) < 28:
                        print("接收到的数据头不完整 数据头是{header.hex()}")
                        continue

                    # 解析数据头
                    command_header = struct.unpack('<H', header[:2])[0]
                    device_id = struct.unpack('<I', header[2:6])[0]
                    command_code = header[6]
                    ext_code = header[7]
                    status_field = header[8:24]
                    data_length = struct.unpack('<I', header[24:28])[0]

                    # print(f"数据头: 命令头 {hex(command_header)}, 设备ID {device_id}, 命令码 {hex(command_code)}, 扩展码 {hex(ext_code)}")

                    # 验证命令头
                    if header[0:2]!=b'\x3c\x3c':  # 雷达数据头
                        print(f"无效的数据头: {header.hex()}")
                        continue

                    # 读取数据部分
                    data = b''
                    if data_length > 0:
                        remaining = data_length
                        while remaining > 0:
                            chunk = self.client_socket.recv(remaining)
                            if not chunk:
                                print("接收数据中断")
                                break
                            data += chunk
                            remaining -= len(chunk)

                    print(f"接收完整指令: {header.hex()}{data.hex()}")
                    # 处理雷达数据
                    radar_data = {
                        'command_header': command_header,
                        'device_id': device_id,
                        'command_code': command_code,
                        'ext_code': ext_code,
                        'status_field': status_field,
                        'data_length': data_length,
                        'data': data
                    }
                    # print(f"接收到雷达数据: {radar_data}")
                    self.handle_radar_data(radar_data)

                except socket.timeout:
                    # 超时是正常的，用于检查是否需要退出循环
                    continue
                except Exception as e:
                    print(f"处理雷达数据时出错: {e}")
                    break

        finally:
            self.client_socket.settimeout(None)
            print("停止监听雷达数据")

    def initialize_radar(self):
        """
        初始化雷达，接收雷达主动上报，更新设备ID等信息
        """
        if not self.client_socket:
            print("雷达未连接")
            return
        current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
        print(f"[Radar][{current_time}] initializing radar")
        self.client_socket.settimeout(1.0)  # 设置超时以便可以检查是否需要退出

        try:
            while True:
                try:
                    # 读取数据头 (前28字节)
                    header = self.client_socket.recv(28)
                    # print(f"接收指令头: {header.hex()}")
                    if not header:
                        print("雷达连接已关闭")
                        break

                    if len(header) < 28:
                        print(f"接收到的数据头不完整 数据头是{header.hex()}")
                        continue

                    # 解析数据头
                    command_header = struct.unpack('<H', header[:2])[0]
                    device_id = struct.unpack('<I', header[2:6])[0]
                    command_code = header[6]
                    ext_code = header[7]
                    status_field = header[8:24]
                    data_length = struct.unpack('<I', header[24:28])[0]


                    # 验证命令头
                    if header[0:2] != b'\x3c\x3c':  # 雷达数据头
                        print(f"无效的数据头: {header.hex()}")
                        continue

                    # 根据数据长度读取数据
                    data = b''
                    if data_length > 0:
                        remaining = data_length
                        while remaining > 0:
                            chunk = self.client_socket.recv(remaining)
                            if not chunk:
                                print("接收数据中断")
                                break
                            data += chunk
                            remaining -= len(chunk)

                    # 处理雷达数据
                    radar_data = {
                        'command_header': command_header,
                        'device_id': device_id,
                        'command_code': command_code,
                        'ext_code': ext_code,
                        'status_field': status_field,
                        'data_length': data_length,
                        'data': data
                    }
                    # print(f"接收指令: {header.hex()}{data.hex()}")
                    current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
                    print(f"[Radar][{current_time}] Received data:{header.hex()}{data.hex()}")

                    self.device_id=device_id

                    if self.device_id:
                        break

                except socket.timeout:
                    # 超时是正常的，用于检查是否需要退出循环
                    continue
                except Exception as e:
                    print(f"处理雷达数据时出错: {e}")
                    break

        finally:
            self.client_socket.settimeout(None)
            current_time = datetime.now().strftime("%H:%M:%S.%f")[:-3]
            print(f"[Radar][{current_time}] initialized radar")


# # 示例用法
# if __name__ == "__main__":
#     radar_server = ASSRRadarServer(port=3000)
#
#     try:
#         # 启动服务器
#         radar_server.start_server()
#         # 初始化雷达状态
#         radar_server.initialize_radar()
#         # # 清空消息队列
#         # radar_server.clear_message_queue()
#
#
#         # 查询雷达状态
#         radar_server.query_status()
#
#         # 查询场景参数
#
#         radar_server.query_scene_params()
#
#         radar_server.query_output_params()
#
#     #
#         # # # 开始工作
#         radar_server.stop_work()
#         # radar_server.query_status()
#         # time.sleep(5)
#         # radar_server.start_work()
#         # #
#         # # time.sleep(10)
#         # radar_server.query_status()
#
#
#
#
#
#
#         # # 启动数据监听线程
#         import threading
#         data_thread = threading.Thread(target=radar_server.listen_for_data, daemon=True)
#         data_thread.start()
#     #
#     #
#     #     #
#     #     # # 查询输出参数
#     #     # if status:
#     #     #     output_params = radar_server.query_output_params()
#     #     #     if output_params:
#     #     #         print("输出参数:", output_params)
#     #     #
#     #     #
#     #     # # 场景参数设置
#     #     # # 设置场景参数 (必须在雷达停止状态下)
#     #     # if status and status['work_state'] == 0:  # 停止工作状态
#     #     #     scene_params = {
#     #     #         'ro_rate': 20,  # 2rpm
#     #     #         'rot_ang_bgn': 0.0,  # 起始角度
#     #     #         'rot_ang_end': 100.0,  # 结束角度
#     #     #         'rng_min': 10,  # 最近探测距离10米
#     #     #         'rng_max': 100,  # 最远探测距离500米
#     #     #         'scan_int': 0,  # 扫描间隔0,不使用
#     #     #         'backup': 0,  # 备用
#     #     #         'atm_switch': 1,  # 开启大气校正
#     #     #         'filter_level': 5,  # 中等滤波
#     #     #         'ps_td': 15  # PS筛选门限
#     #     #     }
#     #     #     radar_server.set_scene_params(scene_params)
#     #     #
#     #     # # 设置输出参数
#     #     # output_params = {
#     #     #     'scat_image_en': 1,  # 开启强度点云
#     #     #     'scat_image_dec': 0,  # 每帧都输出
#     #     #     'defo_image_en': 1,  # 开启形变点云
#     #     #     'defo_image_dec': 0,  # 每帧都输出
#     #     #     'scan_int': 0,  # 扫描间隔60秒
#     #     #     'backup0': 0,     #备用
#     #     #     'backup1': 0,     #备用
#     #     #     'backup2': 0     #备用
#     #     # }
#     #     # radar_server.set_output_params(output_params)
#     #     #
#     #
#     #     # # 查询日志
#     #     # list_log = radar_server.get_log_list()
#     #     #
#     #     # # 获取日志应答
#     #     #
#     #     # for log in list_log:
#     #     #     log_data = radar_server.get_log(log['name'], log['size'])
#     #     #     if log_data:
#     #     #         print(f"日志名称: {log['name']}, 大小: {log['size']}")
#     #     #         print(f"日志内容: {log_data.decode('utf-8')}")
#     #     #
#     #     #
#     #     #
#     #
#     #     # # 开始工作
#     #     # time.sleep(10)
#     #     # radar_server.start_work()
#     #     # # time.sleep(60)
#     #     # # radar_server.stop_work()
#     #     #
#     #     #
#     #     #
#     #     #
#     #     #
#     #     #
#     #     #
#     #     #
#     #
#     #     #
#         # print("开始工作")
#         # radar_server.start_work()
#         #
#         # time.sleep(60)
#         #
#         # print("停止工作")
#         # radar_server.stop_work()
#
#         # 主线程等待
#         while True:
#             time.sleep(1)
#
#     except KeyboardInterrupt:
#         print("接收到中断信号，停止服务器...")
#     finally:
#         # 停止工作
#         radar_server.stop_work()
#         # 停止服务器
#         radar_server.stop_server()
